# /home/zsl/sls_uavs/src/sls_launch_sim/launch/rover_sim.launch.py
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
    TimerAction,
    ExecuteProcess,
)
from launch_ros.actions import Node, SetParameter
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


SetParameter(name='use_sim_time', value=True)


def generate_launch_description():
    ugv_ids = ['ugv1']  # 风格一致
    launch_dir = PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch'])

    return LaunchDescription([
        # 🌍 启动 Gazebo 世界 + 桥接（使用 rover_bridge.yaml）
        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'gzpx.launch.py']),
            launch_arguments={
                'world_sdf': 'default.sdf',  # 你已有的 world
                'bridge_config': '3dslam.yaml'  # 新建的桥接配置
            }.items()
        ),

        # 🤖 Spawn rover_ackermann as ugv1
        IncludeLaunchDescription(
            PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch', 'gzugv.launch.py']),
            launch_arguments={
                'ugv_id': 'ugv1',
                'ugv_pose': '0.0,0.0,0.3,0,0,0',  # z=0.3 防止穿地
            }.items()
        ),

        # 🧭 发布 TF: base_link → velodyne (LiDAR frame)
        # 注意：你的 SDF 中 LiDAR 名为 `/lidar/points`，frame 默认为 sensor 名 → "velodyne"
        TimerAction(
            period=2.0,
            actions=[
                ExecuteProcess(
                    cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher',
                         '--x', '0', '--y', '0', '--z', '0',
                         '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
                         '--frame-id', 'base_link',
                         '--child-frame-id', 'ugv1/lidar_link/lidar_sensor'],
                    output='screen'
                )
            ]
        ),

        # 📦 启动上层节点（如导航、SLAM）
        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'ugv_allstart.launch.py']),
            launch_arguments={
                'ugv_ids': str(ugv_ids)
            }.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'addi_ugv1_bridge.launch.py'])
        ),
                # 🛡️ 启动避障节点（放在 ugv_allstart 之后）
        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'avoidance.launch.py']),
            launch_arguments={
                'obstacle_distance': '2',    # 更激进避障
                'forward_speed': '0.25',
            }.items()
        )
    ])